package ru.ifmo;

import lejos.hardware.Sound;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.Font;
import lejos.hardware.lcd.GraphicsLCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;

import java.io.*;
import java.util.Vector;

/**
 * Created by Admin on 22.01.15.
 */
class SensorThread extends Thread {
    private EV3ColorSensor sensor;
    private GraphicsLCD lcd;
    private float[] rgb = new float[3];
    private SmartSpaceKP kp;
    public static int itemCount = 0;
    private boolean isLogging = false;


    public SensorThread(EV3ColorSensor sensor, GraphicsLCD lcd, SmartSpaceKP kp) {
        this.sensor = sensor;
        this.lcd = lcd;
        this.kp = kp;
    }

    @Override
    public void run() {
        lcd.drawString("SensorThread started!", 9, 10, 0);

        BufferedOutputStream first = null;
        BufferedOutputStream itemData = null;

        Item currentItem = null;

        if (isLogging) {
            try {
                first = new BufferedOutputStream(new FileOutputStream(new File("data.txt")));
            } catch (FileNotFoundException e) {
                e.printStackTrace();
                first = null;
            }
            try {
                itemData = new BufferedOutputStream(new FileOutputStream(new File("items.txt")));
            } catch (FileNotFoundException e) {
                e.printStackTrace();
                itemData = null;
            }
        }

        while (!SortingLine.stopAll) {
            sensor.getRGBMode().fetchSample(rgb, 0);

            float sum = 0.0f;

            for (float it : rgb) {
                sum += it;
            }

            // датчик зарегистрировал объект
            if (sum - 0.03f > 0) {

                // первое появление на датчике предмета
                if (currentItem == null) {
                    currentItem = new Item();
                }

                // апдейтим данные о предмете
                currentItem.increaseComponents(rgb[0], rgb[1], rgb[2]);
            }
            else {
                // предмет пересек датчик полностью
                if (currentItem != null) {

                    final String itemComponentsDescription = String.format("Red: %.2f\nGreen: %.2f\nBlue: %.2f\n",
                                                                        currentItem.getNormalizedRedComponent(),
                                                                        currentItem.getNormalizedGreenComponent(),
                                                                        currentItem.getNormalizedBlueComponent());

                    if (isLogging && itemData != null) {
                        try {
                            itemData.write(itemComponentsDescription.getBytes());
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }

                    new KPInsertDataThread(kp, new SmartSpaceTriplet("Object", "hasColor", currentItem.getColor())).start();

                    currentItem = null; // чистим данные
                }
            }

            if (isLogging && first != null) {
                String firstDesc = String.format("%.5f %.5f %.5f\n", rgb[0], rgb[1], rgb[2]);
                try {
                    first.write(firstDesc.getBytes());
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        }

        sensor.close();


        // вывод даты в файл
        try {
            if (isLogging && itemData != null) {
                Vector<SmartSpaceTriplet> data = kp.query(new SmartSpaceTriplet(null, "hasData", null));
                String answer = "RDF data:\n";
                for (SmartSpaceTriplet triplet : data) {
                    answer += triplet.toString() + "\n";
                }

                try {
                    itemData.write(answer.getBytes());
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }


        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }

        // Выдадим информацию, что предмет можно забирать
        synchronized (kp) {
            try {
                kp.insertData(new SmartSpaceTriplet("Object", "hasStatus", "arrived"));
                Sound.beep();
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }
        }

        if (isLogging) {
            // Закрытие файлов
            try {
                if (first != null) {
                    first.close();
                }
            } catch (IOException e) {
                e.printStackTrace();
            }

            try {
                if (itemData != null) {
                    itemData.close();
                }
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }
}

class MotorThread extends Thread {
    private EV3LargeRegulatedMotor largeMotor;
    private GraphicsLCD lcd;
    private SmartSpaceKP kp;


    MotorThread(EV3LargeRegulatedMotor motor, GraphicsLCD lcd, SmartSpaceKP kp) {
        this.largeMotor = motor;
        this.lcd = lcd;
        this.kp = kp;
    }
    @Override
    public void run() {
        lcd.drawString("motorThread started!", 9, 10, 0);
        largeMotor.setSpeed(200);
        new KPInsertDataThread(kp, new SmartSpaceTriplet("Pipeline", "hasVelocity", Integer.toString(200))).start();
        //largeMotor.setAcceleration(3000);
        largeMotor.rotate((int) (150*8));

        largeMotor.stop();
        new KPUpdateDataThread(kp, new SmartSpaceTriplet("Pipeline", "hasVelocity", Integer.toString(0)), new SmartSpaceTriplet("Pipeline", "hasVelocity", Integer.toString(200))).start();
        SortingLine.stopAll = true;
        lcd.drawString("motorThread finished!", 9, 30, 0);
    }
}


public class SortingLine {
    private EV3 brick;
    private GraphicsLCD lcd;
    private SmartSpaceKP kp;

    private SensorThread  sensorThread;
    private MotorThread motorThread;
    public static volatile boolean stopAll = false;
    private SortingLineHandler handler;

    public SortingLine(EV3 brick) {

        this.brick = brick;

        lcd = this.brick.getGraphicsLCD();
        lcd.clear();
        lcd.setFont(Font.getDefaultFont());
        lcd.drawString("Success!", 9, 1, 0);

        try {
            kp = new SmartSpaceKP("192.168.1.226", 10010, "X", true);
        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }

        handler = new SortingLineHandler(this);



        EV3LargeRegulatedMotor largeMotor = new EV3LargeRegulatedMotor(MotorPort.A);
        EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S4);

        sensorThread = new SensorThread(colorSensor, lcd, kp);
        motorThread = new MotorThread(largeMotor, lcd, kp);

        synchronized (kp) {
            try {
                kp.subscribeData(new SmartSpaceTriplet("Grabber", "hasStatus", "activated"), handler);
                kp.insertData(new SmartSpaceTriplet("SortingLine", "hasStatus", "activated"));
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }
        }

        Sound.twoBeeps();
    }

    public void runCycle() {
        synchronized (kp) {
            try {
                kp.removeData(new SmartSpaceTriplet("SortingLine", "hasStatus", "activated"));
            } catch (SmartSpaceException e) {
                e.printStackTrace();
            }
        }
        motorThread.start();
        sensorThread.start();
    }

    public void leaveSmartSpace() {
        try {
            kp.removeData(new SmartSpaceTriplet("SortingLine", "hasStatus", "activated"));
            kp.leaveSmartSpace();
        } catch (SmartSpaceException e) {
            e.printStackTrace();
        }
    }
}

